// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingMetricsController.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;

    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;

    /// <summary>
    /// Path planning and tracking metrics controller.
    /// </summary>
    public class PathPlanningAndTrackingMetricsController :
        Machine<PathPlanningAndTrackingMetricsControllerStatus.StatusInput,
                PathPlanningAndTrackingMetricsControllerStatus.StatusOutput,
                PathPlanningAndTrackingMetricsControllerStatus>
    {
        /// <summary>
        /// Initializes a new instance of the PathPlanningAndTrackingMetricsController class.
        /// </summary>
        /// <param name="minimumGroundTruthSamplesPerWaypoint">Minimum number of ground truth samples taken at each waypoint.</param>
        /// <param name="groundTruthSampleConsensusThresholdInMillimeters">Threshold of agreement between ground truth samples.</param>
        /// <param name="batteryLowThreshold">Threshold below which we need to dock and charge.</param>
        /// <param name="waypointArrivalThreshold">Threshold at which we consider to have arrived at a waypoint.</param>
        /// <param name="waypoints">Waypoint targets for the metrics run.</param>
        /// <param name="measureGroundTruth">A flag indicating whether to measure ground truth.</param>
        /// <param name="mapResolution">Map resolution in meters.</param>
        /// <param name="usingNavBenchmark">A flag indicating if the nav benchmark is being used. This will modify the initial behavior of the state machine </param>
        public PathPlanningAndTrackingMetricsController(
            int minimumGroundTruthSamplesPerWaypoint,
            double groundTruthSampleConsensusThresholdInMillimeters,
            double batteryLowThreshold,
            double waypointArrivalThreshold,
            Waypoint[] waypoints,
            bool measureGroundTruth,
            double mapResolution,
            bool usingNavBenchmark)
        {
            const double BatteryChargedTheshold = 99;
            const double UndockingSpeed = 0.3;
            const int SecondsToUndockTimeout = 60; // time to wait for the robot to travel required distance.  Some robots stall on the doc (probably a firmware issue), but they should move within 60 seconds.
            const double DistanceToUndock = .1; // how far to travel in meters
            const int SecondsToScan = 22; // allow time for turret scan to complete
            const int MinutesPerWaypointToTimeout = 10;
            const double WaypointApproachThreshold = 0.20;
            const int MillisecondsToWaitForGroundTruthSamples = 60000; // network outages can cause delays during measurement so provide ample time here.
            const int MaxDockingAttempts = 10;
            const int SecondsToAlign = 10; // 8 if faster rotation
            const int SecondsToDock = 12;
            const double DockingSpeed = -0.1;
            
            // read the lanmark in metrics mode
            Func<PathPlanningAndTrackingMetricsControllerStatus, bool>
                measureCheck =
                c => c.Input.GroundTruthMeasurementComplete(minimumGroundTruthSamplesPerWaypoint, groundTruthSampleConsensusThresholdInMillimeters);
            
            Func<PathPlanningAndTrackingMetricsControllerStatus, bool>
                abortCheck =
                c => c.Input.Elapsed.TotalMinutes > MinutesPerWaypointToTimeout || // timeout?
                     c.Input.BatteryCharging; // human placed on dock?

            Func<PathPlanningAndTrackingMetricsControllerStatus, PathPlanningAndTrackingMetricsControllerStatus>
                abortAction =
                c => c.Message(string.Format("Aborted ({0}).", c.Input.BatteryCharging ? "manual docking" : "timeout"))
                      .Stop();

            Func<PathPlanningAndTrackingMetricsControllerStatus, PathPlanningAndTrackingMetricsControllerStatus>
                trackingAction =
                c =>
                {
                    c.Message(string.Format("Tracking (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                    .Drive(c.Input.DriveDirection);
                    c.Output.NavMetrics = c.Input.NavMetrics;
                    c.Output.UpdateMetricsStatus = true;
                    return c;
                };

            Func<PathPlanningAndTrackingMetricsControllerStatus, PathPlanningAndTrackingMetricsControllerStatus>
                nextTargetAction = 
                c => c.SetTarget(c.TargetIndex + 1)
                      .Message(string.Format("Tracking - Next Waypoint (index: {0}, landmark: {1})", c.TargetIndex, c.Waypoints[c.TargetIndex]))
                      .ZeroElapsed()
                      .RecordLocMetrics();

            Func<PathPlanningAndTrackingMetricsControllerStatus, PathPlanningAndTrackingMetricsControllerStatus>
                startDockingAction =
                c => c.Message("Docking...")
                      .RecordDockingAttempt()
                      .ZeroElapsed()
                      .RecordLocMetrics()
                      .RecordNavMetrics();

            // initial state before (or after) a metrics run
            var initial = new State<PathPlanningAndTrackingMetricsControllerStatus>("Initial");

            // battery is charging
            var charging = new State<PathPlanningAndTrackingMetricsControllerStatus>("Charging");

            // rolling off the dock
            var undocking = new State<PathPlanningAndTrackingMetricsControllerStatus>("Undocking");

            // scanning with the turret
            var scanning = new State<PathPlanningAndTrackingMetricsControllerStatus>("Scanning");

            // tracking path plans
            var tracking = new State<PathPlanningAndTrackingMetricsControllerStatus>(
                "Tracking",
                trackingAction);
            
            // approaching waypoint
            var approaching = new State<PathPlanningAndTrackingMetricsControllerStatus>(
                "Approaching",
                trackingAction);
            
            // measuring accuracy based on landmark
            var measuring = new State<PathPlanningAndTrackingMetricsControllerStatus>("Measuring");

            // robot lost.  Probably could not see landmark.
            var lost = new State<PathPlanningAndTrackingMetricsControllerStatus>("FailedToMeasure");

            // aligning and backing onto the dock
            var docking = new State<PathPlanningAndTrackingMetricsControllerStatus>("Docking");

            // failed docking attempt (but "If at first you don't succeed...")
            var failedToDock = new State<PathPlanningAndTrackingMetricsControllerStatus>("Failed");

            // completed successful metrics run!
            var complete = new State<PathPlanningAndTrackingMetricsControllerStatus>("Complete");

            // aborted metrics run (timeout, failed to read landmark, too many docking attempts, ...)
            var aborted = new State<PathPlanningAndTrackingMetricsControllerStatus>(
                "Aborted",
                c =>
                {
                    c.Output.Aborted = true;
                    return c;
                });

            if (!usingNavBenchmark)
            {
                // wait to be docked
                initial.AddTransition(
                    c => !c.Input.BatteryCharging,
                    initial,
                    c => c.Message("Waiting to be manually docked."));

                // ready to begin?
                initial.AddTransition(
                    c => c.Input.BatteryPercentage >= batteryLowThreshold,
                    undocking,
                    c => c.Message("Undocking...")
                          .RecordUndocking()
                          .StartCollectingMetrics()
                          .ZeroElapsed()
                          .ResetDistace()
                          .Drive(double.NaN, UndockingSpeed, 1)); // slowly roll off dock

                // need to charge?
                initial.AddTransition(
                    c => c.Input.BatteryPercentage < batteryLowThreshold,
                    charging);

                // update status while charging
                charging.AddTransition(
                    c => c.Input.BatteryPercentage < BatteryChargedTheshold,
                    charging,
                    c => c.Message(string.Format("Charging ({0})...", c.Input.BatteryPercentage)));

                // done charging?
                charging.AddTransition(c => c.Input.BatteryPercentage >= BatteryChargedTheshold, initial);

                // still undocking?
                undocking.AddTransition(
                    c => (c.Input.Elapsed.TotalSeconds <= SecondsToUndockTimeout) && (c.Input.Distance < DistanceToUndock),
                    undocking,
                    c =>
                    {
                        c.Message("Undocking...")
                        .Drive(double.NaN, UndockingSpeed, 1); // slowly roll off dock
                        c.Output.NavMetrics = c.Input.NavMetrics;
                        c.Output.UpdateMetricsStatus = true;
                        return c;
                    });

                // failed to undock?!
                undocking.AddTransition(
                    c => c.Input.Elapsed.TotalSeconds > SecondsToUndockTimeout &&
                         c.Input.BatteryCharging, // still on the dock?!
                    aborted,
                    c => c.Message("(failed to undock)."));

                // done undocking?
                undocking.AddTransition(
                    c => c.Input.Distance >= DistanceToUndock,
                    tracking,
                    c => c.Message("Tracking")
                        .SetTarget(0)
                        .Stop()
                        .ZeroElapsed());
            }
            else
            {
                // when using the nav benchmark, the robot is placed under the first landmark and we go straight to tracking
                initial.AddTransition(
                    _ => true,
                    tracking,
                    c =>
                        c.Message("Waiting for NavBenchamrk reply to Start Message")
                        .StartCollectingMetrics()
                        .ZeroElapsed()
                        .ResetDistace()
                        .SetTarget(0));
            }

            // abort after long timeout (must be stuck) or robot manually placed on dock
            tracking.AddTransition(abortCheck, aborted, abortAction);

            // keep tracking until reaching approach distance
            tracking.AddTransition(
                c => c.ProximityToTarget(mapResolution) > WaypointApproachThreshold,
                tracking, 
                trackingAction);

            // Scan if within approach distance
            tracking.AddTransition(
                c => c.ProximityToTarget(mapResolution) <= WaypointApproachThreshold,
                scanning,
                c => c.Message(string.Format("Scanning (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                      .InitiateTurretScan());

            // done scanning? Go to Approaching
            scanning.AddTransition(
                c => c.Input.Elapsed.TotalSeconds > SecondsToScan,
                approaching,
                c => c.Message(string.Format("Approacing (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                      .ZeroElapsed());

            // abort after long timeout (must be stuck) or robot manually placed on dock
            approaching.AddTransition(abortCheck, aborted, abortAction);

            // keep tracking until arriving at waypoint
            approaching.AddTransition(
                c => c.ProximityToTarget(mapResolution) > waypointArrivalThreshold,
                approaching, 
                trackingAction);

            if (!usingNavBenchmark)
            {
                // arrived at a measurable waypoint and we are measuring ground truth
                approaching.AddTransition(
                    c => measureGroundTruth && c.CurrentTarget.IsMarked && c.ProximityToTarget(mapResolution) <= waypointArrivalThreshold,
                    measuring,
                    c => c.Message(string.Format("Measuring (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                          .Stop()
                          .ResetGroundTruthSamples()
                          .ZeroElapsed()
                          .SignalLandmarkArrival());

                // arrived at a measurable waypoint and we are not measuring ground truth
                // signal arrival and then proceed to next landmark.
                approaching.AddTransition(
                    c => !measureGroundTruth && c.CurrentTarget.IsMarked && c.ProximityToTarget(mapResolution) <= waypointArrivalThreshold,
                    tracking,
                    c =>
                    {
                        c.Message(string.Format("Signalling Arrival (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                        .Stop()
                        .SignalLandmarkArrival();
                        return nextTargetAction(c);
                    });

                // arrived at a dock with no landmark. go straight to docking
                approaching.AddTransition(
                    c => c.ProximityToTarget(mapResolution) <= waypointArrivalThreshold &&
                        !c.CurrentTarget.IsMarked && c.CurrentTarget.IsDock,
                    docking,
                    startDockingAction);

                // allow only so much time for good samples to arrive
                measuring.AddTransition(
                    c => c.Input.Elapsed.TotalMilliseconds > MillisecondsToWaitForGroundTruthSamples,
                    lost,
                    c => c.Message(string.Format("Failed to Measure (Time expired without waypoint marker seen). (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage)));

                // keep measuring until the measure check passes or timeout (above)
                measuring.AddTransition(
                    c => !measureCheck(c),
                    measuring);

                // succeeded to measure, not at dock, start tracking to next waypoint
                measuring.AddTransition(
                    c => !c.CurrentTarget.IsDock && measureCheck(c),
                    tracking,
                    nextTargetAction);

                // succeeded to measure at a dock, start docking
                measuring.AddTransition(
                    c => c.CurrentTarget.IsDock && measureCheck(c),
                    docking,
                    startDockingAction);
            }
            else
            {
                // using nav benchmark.
                // arrived at a waypoint, signal benchmark to measure
                approaching.AddTransition(
                    c => c.ProximityToTarget(mapResolution) <= waypointArrivalThreshold,
                    measuring,
                    c => c.Stop()
                        .SignalLandmarkArrival()); // this will cause a blocking call to nav benchmark 

                // after measuring go to tracking if there are more waypoints.
                measuring.AddTransition(
                    c => c.TargetIndex < c.Waypoints.Length - 1, 
                    tracking, 
                    c => c.SetTarget(c.TargetIndex + 1)); // this will cause a blocking call to nav benchmark

                // after measuring.  No more landmarks. end
                measuring.AddTransition(
                    _ => true,
                    complete,
                    c => c.Message(string.Format("Completed NavBenchamrk (index:{0} landmark:{1} proximity:{2:0.000} battery:{3})", c.TargetIndex, waypoints[c.TargetIndex].LandmarkId, c.ProximityToTarget(mapResolution), c.Input.BatteryPercentage))
                          .Stop());

                complete.AddTransition(_ => true, complete);
            }

            lost.AddTransition(_ => true, aborted); // FIXME: just always go to aborted when lost for now

            // line up with the dock (heading adjustment only)
            docking.AddTransition(
                c => c.Input.Elapsed.TotalSeconds < SecondsToAlign,
                docking,
                c => c.Message("Docking - Aligning")
                      .Drive(c.CurrentTarget.Pose.Heading, 0, 1)); // docking pose adjustment (heading only)

            // backing onto dock
            docking.AddTransition(
                c => c.Input.Elapsed.TotalSeconds < SecondsToAlign + SecondsToDock,
                docking,
                c => c.Message("Docking - Backing")
                      .Drive(double.NaN, DockingSpeed, 1)); // backing slowly

            // docked successfully?
            docking.AddTransition(
                c => c.Input.Elapsed.TotalSeconds >= SecondsToAlign + SecondsToDock &&
                     c.Input.BatteryCharging,
                complete,
                c => c.Message("Docking - Success")
                      .Stop());

            // docking failed?
            docking.AddTransition(
                c => c.Input.Elapsed.TotalSeconds >= SecondsToAlign + SecondsToDock &&
                     !c.Input.BatteryCharging,
                failedToDock,
                c => c.Message("Docking - Failure")
                      .Stop());

            // too many attempts to dock... :(
            failedToDock.AddTransition(
                c => c.Output.DockingAttempts >= MaxDockingAttempts, // we've tried too many times
                aborted,
                c => c.Message("(too many failed docking attempts)."));

            // try, try, try again...
            failedToDock.AddTransition(
                c => c.Output.DockingAttempts < MaxDockingAttempts,
                undocking, // we call it 'undocking', but more likely 'untangle-from-partial-docking'
                c => c.SetTarget(Math.Max(0, c.Waypoints.Length - 2)) // backtrack a bit to relocalize and retry docking
                      .Drive(double.NaN, -DockingSpeed, 1) // slowly roll forward
                      .ZeroElapsed());

            if (!usingNavBenchmark)
            {
                // we're done! again, again!
                complete.AddTransition(
                    _ => true,
                    initial,
                    c =>
                    {
                        c.Output.Message = "Navigation Run Complete!";
                        c.Output.DockingAttempts = 0;
                        c.TargetIndex = 0;
                        return c;
                    });
            }

            var status = new PathPlanningAndTrackingMetricsControllerStatus(waypoints);
            status.SetTime(DateTime.Now);
            status.ZeroElapsed();
            this.Initialize(status, initial);
        }

        /// <summary>
        /// Update state machine with drive direction (presumably from path tracking).
        /// </summary>
        /// <param name="drive">Drive direction.</param>
        /// <param name="time">Current time.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateDrive(DriveDirection drive, DateTime time)
        {
            this.Context.SetTime(time);
            this.Context.Input.DriveDirection = drive;
            return this.Update(this.Context.Input);
        }

        /// <summary>
        /// Update state machine with metrics information.
        /// </summary>
        /// <param name="metrics">Metrics information.</param>
        /// <param name="time">Current time.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateNavMetrics(PathPlanningAndTrackingMetrics metrics, DateTime time)
        {
            this.Context.SetTime(time);
            this.Context.Input.NavMetrics = metrics;
            return this.Update(this.Context.Input);
        }

        /// <summary>
        /// Update state machine with localization ground truth metrics.
        /// </summary>
        /// <param name="relativePose">Robot pose relative to the landmark.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateLocMetrics(Pose2D relativePose)
        {
            this.Context.SetLocalizationGroundTruth(relativePose);
            return this.Update(this.Context.Input);
        }

        /// <summary>
        /// Update state machine with battery information.
        /// </summary>
        /// <param name="charging">Whether currently charging the battery.</param>
        /// <param name="percentage">Current battery charge percentage.</param>
        /// <param name="time">Current time.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateBattery(bool charging, double percentage, DateTime time)
        {
            this.Context.SetTime(time);
            this.Context.SetBatteryState(charging, percentage);
            return this.Update(this.Context.Input);
        }

        /// <summary>
        /// Update state machine with drive feedback information.
        /// </summary>
        /// <param name="driveFeedbackState">The drive feedback state.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateDistance(DriveFeedbackState driveFeedbackState)
        {
            this.Context.SetDistance(driveFeedbackState);
            return this.Update(this.Context.Input);
        }
        
        /// <summary>
        /// Update time.
        /// </summary>
        /// <param name="time">Current time.</param>
        /// <returns>Updated context output.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus.StatusOutput UpdateTime(DateTime time)
        {
            this.Context.SetTime(time);
            return this.Update(this.Context.Input);
        }

        /// <summary>
        /// String representation for debugging and testing.
        /// </summary>
        /// <returns>String representation.</returns>
        public override string ToString()
        {
            return string.Format("State:{0}", this.State);
        }
    }
}
